#include "./motors.h"
#include "../xlib/gpio.h"
#include "./sensor.h"
#include "../rtx/rtx_os.h"

namespace main_driver {

Motors motors;

namespace motors_impl {

using namespace stm32f1;
using namespace gpio;

using DIR1 = PC<8>;
using DIR2 = PC<9>;
using PICK1 = PC<10>;
using PICK2 = PC<11>;
using STRAW = PC<12>;
using FILM = PC<13>;

enum class Dir : uint8_t {
    CW = 0,
    CCW = 1,
    BRAKE = 2,
};

static void set_dir(Dir dir) {
    switch (dir) {
        case Dir::CW: 
            DIR1::set();
            DIR2::clr();
        break;

        case Dir::CCW:
            DIR1::clr();
            DIR2::set();
        break;
    
        default:
            DIR1::clr();
            DIR2::clr();
        break;
    }
}

}

using namespace motors_impl;

void Motors::init() {

    rcc::enable<
        DIR1, DIR2, PICK1, PICK2,
        STRAW, FILM
    >();

    config<
        Cfg<DIR1, Mode::OUT_50MHz_PP>,
        Cfg<DIR2, Mode::OUT_50MHz_PP>,
        Cfg<PICK1, Mode::OUT_50MHz_PP>,
        Cfg<PICK2, Mode::OUT_50MHz_PP>,
        Cfg<STRAW, Mode::OUT_50MHz_PP>,
        Cfg<FILM, Mode::OUT_50MHz_PP>
    >();

    DIR1::clr();
    DIR2::clr();
    PICK1::clr();
    PICK2::clr();
    STRAW::clr();
    FILM::clr();
}

Error Motors::pick1_open() {
    Error err;
    uint32_t time_count = 0;
    uint32_t lock_count = 0;

    if (sensor.is_pick1_right()) {
        goto finish;
    }
    
    set_dir(Dir::CW);
    PICK1::set();

    for (;;) {
        if (sensor.is_pick1_right()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= PICK_ALM)) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::main::PICK1_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }

        osDelay(50);
        time_count += 50;
        if (time_count >= PICK_TIMEOUT) {
            err = ec::main::PICK1_TIMEOUT;
            goto finish;
        }    
    }

finish:
    set_dir(Dir::BRAKE);
    PICK1::clr();
    return err;
}

Error Motors::pick1_close() {
    Error err;
    uint32_t time_count = 0;
    uint32_t lock_count = 0;
    if (sensor.is_pick1_left()) {
        goto finish;
    }
    
    set_dir(Dir::CCW);
    PICK1::set();

    for (;;) {
        if (sensor.is_pick1_left()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= PICK_ALM)) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::main::PICK1_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }

        osDelay(50);
        time_count += 50;
        if (time_count >= PICK_TIMEOUT) {
            err = ec::main::PICK1_TIMEOUT;
            goto finish;
        }    
    }

finish:
    set_dir(Dir::BRAKE);
    PICK1::clr();
    return err;
}

Error Motors::pick2_open() {
    Error err;
    uint32_t time_count = 0;
    uint32_t lock_count = 0;
    if (sensor.is_pick2_right()) {
        goto finish;
    }
    
    set_dir(Dir::CW);
    PICK2::set();

    for (;;) {
        if (sensor.is_pick2_right()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= PICK_ALM)) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::main::PICK1_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }

        osDelay(50);
        time_count += 50;
        if (time_count >= PICK_TIMEOUT) {
            err = ec::main::PICK2_TIMEOUT;
            goto finish;
        }    
    }

finish:
    set_dir(Dir::BRAKE);
    PICK2::clr();
    return err;
}

Error Motors::pick2_close() {
    Error err;
    uint32_t time_count = 0;
    uint32_t lock_count = 0;
    if (sensor.is_pick2_left()) {
        goto finish;
    }
    
    set_dir(Dir::CCW);
    PICK2::set();

    for (;;) {
        if (sensor.is_pick2_left()) {
            break;
        }

        if ((time_count >= 500) && (sensor.adc_value() >= PICK_ALM)) {
            lock_count ++;
            if (lock_count >= 5) {
                err = ec::main::PICK1_LOCKED;
                goto finish;
            }
        } else {
            lock_count = 0;
        }

        osDelay(50);
        time_count += 50;
        if (time_count >= PICK_TIMEOUT) {
            err = ec::main::PICK2_TIMEOUT;
            goto finish;
        }    
    }

finish:
    set_dir(Dir::BRAKE);
    PICK2::clr();
    return err;
}

Error Motors::film() {
    Error err;
    uint32_t time_count = 0;
    if (!sensor.is_film_checked()) {
        err = ec::main::FILM_NOT_CUP;
        goto finish;
    }
    set_dir(Dir::CW);
    FILM::set();
    osDelay(800);
    for (;;) {
        if (sensor.is_film_front()) {
            break;
        }
        osDelay(50);
        time_count += 50;
        if (time_count >= FILM_TIMEOUT) {
            err = ec::main::FILM_TIMEOUT;
            goto finish;
        }
    }
finish:
    set_dir(Dir::BRAKE);
    FILM::clr();
    return err;
}

Error Motors::straw_open() {
    Error err;

    set_dir(Dir::CCW);
    STRAW::set();

    osDelay(500);

    set_dir(Dir::BRAKE);
    STRAW::clr();

    return err;
}

Error Motors::straw_close() {
    Error err;

    if (!sensor.is_straw_checked()) {
        err = ec::main::STRAW_CLOSE_FAIL;
        goto finish;
    }

    set_dir(Dir::CW);
    STRAW::set();
    osDelay(500);
    set_dir(Dir::BRAKE);
    STRAW::clr();

finish:
    return err;
}

}
